clc
clear all
%%机器人模型导入
%urdf模型导入
robot = importrobot("/home/maoli/Desktop/Dynamic_ParaEstimation_v3/leg_v3_left_serial(1).urdf");
% robot = importrobot("/home/maoli/Desktop/Dynamic_ParaEstimation_v3/leg_v3_right_serial.urdf");

% robot = importrobot("/home/maoli/Desktop/biped_v2_calib/xacro/leg_v2_serial_left.urdf");
robot.DataFormat='column';
robot.Gravity =[0 0 -9.81];%%



%%根据urdf模型确定刚体系统自由度
N_fixed = 0;
for i = 1:1:robot.NumBodies
    if( strcmp(robot.Bodies{1, i}.Joint.Type,'fixed') == true)
        N_fixed=N_fixed+1;
    end   
end
N_dof = robot.NumBodies - N_fixed;

%创建机器人刚体系统参数结构体：robot_para
robot_para.JointAxis = zeros(3,N_dof);
robot_para.JointToParentTransform = zeros(4,4,N_dof);
for i = 1:1:N_dof
    robot_para.JointAxis(:,i) = robot.Bodies{1,i+1}.Joint.JointAxis;
    robot_para.JointToParentTransform(:,:,i) = robot.Bodies{1,i+1}.Joint.JointToParentTransform;
end

%定义连杆的惯量 N_dof个自由度的连杆
I_c=zeros(3,3,N_dof);
I_o=zeros(3,3,N_dof);
d_oc=zeros(3,N_dof);
m=zeros(1,N_dof); %质量

%根据所导入的urdf模型读取各连杆的质量 惯量（相对于各刚体坐标系原点） 和质心位置向量（相对于各个刚体坐标系原点）
for i = 1:1:N_dof
    m(i) = robot.Bodies{1, i+1}.Mass; %由于urdf模型中，第一个刚体为固定基，因此第一个活动连杆的索引为2， 也是循环i+1的由来
    d_oc(:,i) = robot.Bodies{1, i+1}.CenterOfMass';
end

%模型得到的动力学参数集合 param =[mi micx micy micz i_xx i_xy i_xz i_yy i_yz i_zz]
param_model_matrix=zeros(10,N_dof);
for i=1:N_dof
    param_model_matrix(1,i)=m(i);
    param_model_matrix(2:4,i)=m(i)*d_oc(:,i);
    %%从urdf中导入各连杆相对于对应关节坐标系的转动惯量
    param_model_matrix(5,i)=robot.Bodies{1,i+1}.Inertia(1);% I_xx
    param_model_matrix(6,i)=robot.Bodies{1,i+1}.Inertia(6);% I_xy
    param_model_matrix(7,i)=robot.Bodies{1,i+1}.Inertia(5);% I_xz
    param_model_matrix(8,i)=robot.Bodies{1,i+1}.Inertia(2);% I_yy
    param_model_matrix(9,i)=robot.Bodies{1,i+1}.Inertia(4);% I_yz
    param_model_matrix(10,i)=robot.Bodies{1,i+1}.Inertia(3);% I_zz
end
Para_vec=zeros(N_dof*10,1);
for i = 1:1:N_dof
    Para_vec(10*(i-1)+1:10*i)=param_model_matrix(:,i);
end

%%计算惯量权重
omega_I = zeros(5*N_dof,1);
for i = 1:1:N_dof
    I_o(1,1,i) = param_model_matrix(5,i);
    I_o(1,2,i) = param_model_matrix(6,i);
    I_o(2,1,i) = param_model_matrix(6,i);
    I_o(1,3,i) = param_model_matrix(7,i);
    I_o(3,1,i) = param_model_matrix(7,i);
    I_o(2,2,i) = param_model_matrix(8,i);
    I_o(2,3,i) = param_model_matrix(9,i);
    I_o(3,2,i) = param_model_matrix(9,i);
    I_o(3,3,i) = param_model_matrix(10,i);
    
end


for i = 1:1:N_dof
    omega_I(10*(i-1)+1) = 1/Para_vec(10*(i-1)+1);
%     omega_I(10*(i-1)+2) = 1/(norm(Para_vec(10*(i-1)+2))+norm(Para_vec(10*(i-1)+3))+norm(Para_vec(10*(i-1)+4)));
%     omega_I(10*(i-1)+3) = 1/(norm(Para_vec(10*(i-1)+2))+norm(Para_vec(10*(i-1)+3))+norm(Para_vec(10*(i-1)+4)));
%     omega_I(10*(i-1)+4) = 1/(norm(Para_vec(10*(i-1)+2))+norm(Para_vec(10*(i-1)+3))+norm(Para_vec(10*(i-1)+4)));
    omega_I(10*(i-1)+2) = 1/(norm([Para_vec(10*(i-1)+2) Para_vec(10*(i-1)+3) Para_vec(10*(i-1)+4)]));
    omega_I(10*(i-1)+3) = 1/(norm([Para_vec(10*(i-1)+2) Para_vec(10*(i-1)+3) Para_vec(10*(i-1)+4)]));
    omega_I(10*(i-1)+4) = 1/(norm([Para_vec(10*(i-1)+2) Para_vec(10*(i-1)+3) Para_vec(10*(i-1)+4)]));
    
    [V,D] =eig(I_o(:,:,i));
%     trace(D)
    omega_I(10*(i-1)+5) = 1/trace(D);
    omega_I(10*(i-1)+6) = 1/trace(D);
    omega_I(10*(i-1)+7) = 1/trace(D);
    omega_I(10*(i-1)+8)  = 1/trace(D);
    omega_I(10*(i-1)+9)  = 1/trace(D);
    omega_I(10*(i-1)+10) = 1/trace(D);  
      
end
omega_I = omega_I/omega_I(1);

%%数据读取
% data = xlsread('C:\Users\Admin\Desktop\Dynamic_ParaEstimation\2023-09-26.12时18分25秒_log.csv',1,'A1:O19999')
% data1 = readtable('/home/maoli/Desktop/Dynamic_ParaEstimation/real_leg_cali.csv');
% % data1 = readtable('C:\Users\Admin\Desktop\Dynamic_ParaEstimation\2023-09-26.12时18分25秒_log.csv');
% data = table2array(data1);
% data_q = data(:,1:5)';
% data_qd = data(:,6:10)';
% data_tau = data(:,11:15)';

data1 = readtable('/home/maoli/Desktop/Dynamic_ParaEstimation_v3/real_leg3_cali_4.csv');
data = table2array(data1);
data_q = data(:,1:5)';
data_qd = data(:,11:15)';
data_tau = data(:,21:25)';
% data_q = data(:,6:10)';
% data_qd = data(:,16:20)';
% data_tau = data(:,26:30)';
data_qd_raw = data_qd;
data_q_raw = data_q;


t = 0.002;%采样周期 0.002s
N_data =size(data_q,2);
data_qdd = zeros(5,N_data - 2);
data_qd = zeros(5,N_data - 1);

data_qdd_raw = zeros(5,N_data - 2);
for i = 1:N_data -2
    data_qdd_raw(:,i) = (data_qd_raw(:,i+1) - data_qd_raw(:,i))/t; 
end
alpha = 0.05;
data_qdd_raw_filted_ = data_qdd_raw;
data_qd_raw_filted_ = data_qd_raw;
data_q_raw_filted_ = data_q_raw;
data_tau_raw_filted_ =data_tau;
for i = 1:N_data -3
    data_qdd_raw_filted_(:,i+1) = alpha*data_qdd_raw_filted_(:,i+1) + (1-alpha)*data_qdd_raw_filted_(:,i);
    data_qd_raw_filted_(:,i+1) = alpha*data_qd_raw_filted_(:,i+1) + (1-alpha)*data_qd_raw_filted_(:,i);
    data_q_raw_filted_(:,i+1) = alpha*data_q_raw_filted_(:,i+1) + (1-alpha)*data_q_raw_filted_(:,i);
    data_tau_raw_filted_(:,i+1) = alpha*data_tau_raw_filted_(:,i+1) + (1-alpha)*data_tau_raw_filted_(:,i);

end

for i = 1:N_data -1
    data_qd(:,i) = (data_q(:,i+1) - data_q(:,i))/t; 
end
for i = 1:N_data -2
    data_qdd(:,i) = (data_qd(:,i+1) - data_qd(:,i))/t; 
end
alpha = 0.05;
data_qdd_filted_ = data_qdd;
data_qd_filted_ = data_qd;
data_q_filted_ = data_q;
data_tau_filted_ =data_tau;
for i = 1:N_data -3
    data_qdd_filted_(:,i+1) = alpha*data_qdd_filted_(:,i+1) + (1-alpha)*data_qdd_filted_(:,i);
    data_qd_filted_(:,i+1) = alpha*data_qd_filted_(:,i+1) + (1-alpha)*data_qd_filted_(:,i);
    data_q_filted_(:,i+1) = alpha*data_q_filted_(:,i+1) + (1-alpha)*data_q_filted_(:,i);
    data_tau_filted_(:,i+1) = alpha*data_tau_filted_(:,i+1) + (1-alpha)*data_tau_filted_(:,i);

end

% data_qdd_filted_(:,1:9988)=data_qdd_filted_(:,11:9998);
% data_qd_filted_(:,1:9988)=data_qd_filted_(:,11:9998);
% data_q_filted_(:,1:9988)=data_q_filted_(:,11:9998);
% data_tau_filted_(:,1:9988)=data_tau_filted_(:,11:9998);

data_qdd_filted_(:,1:9898)=data_qdd_filted_(:,101:9998);
data_qd_filted_(:,1:9898)=data_qd_filted_(:,101:9998);
data_q_filted_(:,1:9898)=data_q_filted_(:,101:9998);
data_tau_filted_(:,1:9898)=data_tau_filted_(:,101:9998);
data_tau(:,1:9898)=data_tau(:,101:9998);



K = zeros(N_dof,N_dof*10,N_data - 1);
K_aug = zeros(N_dof,N_dof*12,N_data - 1);

K_raw = zeros(N_dof,N_dof*10,N_data - 1);
K_aug_raw = zeros(N_dof,N_dof*12,N_data - 1);

for i = 1: N_data - 2
    [U,Uj] = Compute_LinearDynmatrix(data_q_filted_(:,i), data_qd_filted_(:,i), data_qdd_filted_(:,i),robot_para);
    K(:,:,i) = Uj;
    
    K_aug(:,1:N_dof*10,i)=Uj;
    for j = 1:N_dof
%         K_aug(j,N_dof*10 + (j-1)*2 + 1,i) = sign(data_qd_filted_(j,i));  %Bc*sign(joint_velocity)
        gain_k=10;
        K_aug(j,N_dof*10 + (j-1)*2 + 1,i) = tanh(gain_k * data_qd_filted_(j,i));  %Bc*tanh(k * joint_velocity) 摩擦力换向平滑        
        K_aug(j,N_dof*10 + (j-1)*2 + 2,i) = data_qd_filted_(j,i);  % B*joint_velocity        
%         K_aug(j,N_dof*10 + (j-1)*2 + 1,i) = sign(data_qd(j,i));  %Bc*sign(joint_velocity)
%         K_aug(j,N_dof*10 + (j-1)*2 + 2,i) = data_qd(j,i);  % B*joint_velocity
    end
end

for i = 1: N_data - 2
    [U,Uj] = Compute_LinearDynmatrix(data_q_raw_filted_(:,i), data_qd_raw_filted_(:,i), data_qdd_raw_filted_(:,i),robot_para);
    K(:,:,i) = Uj;
    
    K_aug_raw(:,1:N_dof*10,i)=Uj;
    for j = 1:N_dof
        K_aug_raw(j,N_dof*10 + (j-1)*2 + 1,i) = sign(data_qd_filted_(j,i));  %Bc*sign(joint_velocity)
        K_aug_raw(j,N_dof*10 + (j-1)*2 + 2,i) = data_qd_filted_(j,i);  % B*joint_velocity        
%         K_aug(j,N_dof*10 + (j-1)*2 + 1,i) = sign(data_qd(j,i));  %Bc*sign(joint_velocity)
%         K_aug(j,N_dof*10 + (j-1)*2 + 2,i) = data_qd(j,i);  % B*joint_velocity
    end
end
%%数据预处理及线性动力学矩阵计算完成
%%*****************************************%%
%                                           %
%%*****************************************%%

%%开始进行优化建模及求解
nb=size(data_tau,1);
n_data = 5000; %小于19998的数

%%cali.csv
%%右腿
% X     = [0.571899999441041;-0.00051470999999950;-2.41091409592109e-19;0.00177288999999976;0.000649329958962630;-1.19999999915586e-07;5.57760099747065e-06;0.000395865198052475;5.79999999953058e-08;0.000356162638037552;0.571807244259609;0.00177476231812208;-0.0104149386160814;2.96113089355272e-05;0.000546042743485654;2.85386841337639e-05;3.14525033430812e-06;0.000649164355478095;3.38205516374512e-06;0.000588757837421502;1.43270200463161;0.00421479494835939;-0.00111243655947666;-0.0194333070784148;0.00361937998962231;5.61119118504848e-05;-0.000150874618714729;0.00343964394022792;2.52992407496316e-06;0.00183361179113872;0.292718582279548;0.00355055106300956;0.000710877400403101;-0.0240897508582548;0.00503997058625096;-2.64923095646526e-05;0.000486213030180104;0.00504682200179224;0.000499515737768853;0.000150674662312278;0.124337981727035;0.00348976122681330;0.000143369737828755;-0.00515036682925516;0.000286378454941797;3.60758735441360e-06;0.000111713080149110;0.000487274735841030;3.63369881198498e-06;0.000223817890325416;0.0932936216586037;3.03816982577849e-11;0.109537676123579;0.0668600329170273;0.136138979154918;0.0565787154754515;0.149015497631690;0.0635423868337989;0.0255812742773446;0.0892550112621947];
% X_raw = [0.571900000000000;-0.000514710000000000;0;0.00177289000000000;0.000649329959000000;-1.20000000000000e-07;5.57760100000000e-06;0.000395865198000000;5.80000000000000e-08;0.000356428239000000;0.571900000000000;0.00177289000000000;-0.0105229600000000;0;0.000549587464000000;2.86391760000000e-05;5.80000000000000e-08;0.000649329959000000;1.20000000000000e-07;0.000589024423000000;1.34580000000000;0.00215328000000000;-0.00134580000000000;-0.0174954000000000;0.00370501600000000;3.58228000000000e-06;-0.000188221360000000;0.00346547344800000;-3.58964000000000e-05;0.00184487804800000;0.245000000000000;0.00475275500000000;-4.01800000000000e-05;-0.0298802000000000;0.00504095578152000;5.29451820000000e-07;0.000588972999800000;0.00517846488624500;-4.97935280000000e-06;0.000194923283765000;0.131856000000000;0.00338368867200000;0;-0.00554797305600000;0.000286842514304256;0;0.000112106084563072;0.000487413733005120;4.47000000000000e-10;0.000224543218700864;0.0541526795194433;9.69221017872543e-11;0.0585168149031637;0.0189019411093441;0.101431488795825;0.00170374147663682;0.117323384204510;0.0342703864052806;0.00870769645781322;0.0423461865159219];
%%左腿
X = [0.571899997722808;-0.000514710000680556;3.45758842820986e-16;0.00177289000019663;0.000649330052812348;-1.20000194079396e-07;5.57760694897879e-06;0.000395865085048964;5.80000079452408e-08;0.000364644544245487;0.574769385950991;0.00172926385088370;0.0105232417263631;-8.59377371711860e-06;0.000548754607626911;-2.81069230096837e-05;-3.03851927283841e-06;0.000649489873365614;7.60804520766972e-07;0.000602234335795308;1.40357107859845;0.00348893402982374;0.00115876323143757;-0.0185617224735223;0.00384910596597239;6.80473442465877e-05;-6.66520127720329e-05;0.00390285532809723;4.90023470532199e-05;0.00201548278737801;0.297697818253264;0.00629656946893512;-0.000484661949421056;-0.0268444689473700;0.00621036612556600;0.000126385978341183;0.000452947154693881;0.00738021525907734;0.000102384642114500;0.00120646921658416;0.0882681193551178;0.00109742106445608;-3.56653084913920e-07;-0.00312407321909837;0.000188460579192084;1.00904732071194e-06;6.89576439673875e-05;0.000274148544773374;1.71367860680723e-06;8.58552185408497e-05;0.103035886368801;0.0306080628304404;0.185921044948834;0.0301906270301812;0.0793042157978555;0.0288892407568089;0.180192255709923;0.0630274948186434;0.0563358472214242;0.0691190109066281];%tau_filted
% X= [0.571899998175447;-0.000514710000391459;2.12218811217447e-16;0.00177289000014268;0.000649330010298712;-1.20000108795159e-07;5.57760446280417e-06;0.000395865132934877;5.80000078764125e-08;0.000375474905257675;0.578551634962699;0.00167175888284442;0.0106186346057364;4.02577240472938e-05;0.000559336559538533;-3.07814495856146e-05;-1.10535478988954e-05;0.000650770863342560;1.08628697304976e-05;0.000618576567573759;1.39863806438852;0.00367090007165325;0.00101204136744728;-0.0181343640060400;0.00411673059339095;8.29276034566679e-05;-0.000130031195971789;0.00425756490934438;0.000113849166152225;0.00243240030420225;0.297668046626713;0.00560332651014265;-0.000711593849372530;-0.0256571128499172;0.00693723417870505;0.000224496880989606;0.000236584422880800;0.00833746760957740;0.000248805399284577;0.00150171405479899;0.0917286905928955;0.00106601608406504;1.93696220760882e-06;-0.00319785534305000;0.000191661735088730;3.20489041623244e-06;6.87490613126275e-05;0.000279192743105910;1.75755919117186e-06;8.80837977862381e-05;0.0880318514891883;7.60701830813387e-10;0.176580125438312;1.11352171844787e-09;0.0662624672034370;7.30747285867218e-10;0.191298001986496;0.0349825095143135;0.0883381412078764;0.0388524735019607];%%tau
X_raw = [0.571899991706159;-0.000514709999991247;-4.27660876402098e-18;0.00177288999999617;0.000649329957923418;-1.19999997633454e-07;5.57760092926597e-06;0.000395865199442562;5.79999998917698e-08;0.000356170273960667;0.571809902852654;0.00177425970683382;-0.0104377628860146;3.07764441424009e-05;0.000546021048901282;2.84915723750896e-05;4.52888474340825e-06;0.000649201576756843;3.35739932955378e-06;0.000588733039651754;1.43729089664814;0.00386351825748708;-0.00117050951040018;-0.0190044915543203;0.00362808836050562;4.98124346935048e-05;-0.000111166927269954;0.00343671941941008;-3.06400820245648e-05;0.00182372124702723;0.297675935778451;0.00382056344368170;0.000496849902999328;-0.0246743539422835;0.00504469508154872;-5.16982881771652e-06;0.000458313113886417;0.00508367861399015;0.000459154488903580;0.000177374900041682;0.122523685948498;0.00344329165592477;7.11067046347063e-05;-0.00530533132230981;0.000286217719908084;3.82120767329781e-06;0.000111711082552886;0.000487739790932843;3.55465142651782e-06;0.000223964142086207;0.107493907859007;1.87794681798331e-11;0.161967446369702;1.50061314262342e-09;0.185033593159880;0.0200768756033842;0.186544337859972;0.0436990118536193;0.0453097700286946;0.0624762724266211];

% X     = [0.571900000000000;-0.000514710000000000;0;0.00177289000000000;0.000649329959000000;-1.20000000000000e-07;5.57760100000000e-06;0.000395865198000000;5.80000000000000e-08;0.000356428239000000;0.571900000000000;0.00177289000000000;-0.0105229600000000;0;0.000549587464000000;2.86391760000000e-05;5.80000000000000e-08;0.000649329959000000;1.20000000000000e-07;0.000589024423000000;1.34580000000000;0.00215328000000000;-0.00134580000000000;-0.0174954000000000;0.00370501600000000;3.58228000000000e-06;-0.000188221360000000;0.00346547344800000;-3.58964000000000e-05;0.00184487804800000;0.245000000000000;0.00475275500000000;-4.01800000000000e-05;-0.0298802000000000;0.00504095578152000;5.29451820000000e-07;0.000588972999800000;0.00517846488624500;-4.97935280000000e-06;0.000194923283765000;0.131856000000000;0.00338368867200000;0;-0.00554797305600000;0.000286842514304256;0;0.000112106084563072;0.000487413733005120;4.47000000000000e-10;0.000224543218700864;0;0;0;0;0;0;0;0;0;0];
% X_raw = [0.571900000000000;-0.000514710000000000;0;0.00177289000000000;0.000649329959000000;-1.20000000000000e-07;5.57760100000000e-06;0.000395865198000000;5.80000000000000e-08;0.000356428239000000;0.571900000000000;0.00177289000000000;-0.0105229600000000;0;0.000549587464000000;2.86391760000000e-05;5.80000000000000e-08;0.000649329959000000;1.20000000000000e-07;0.000589024423000000;1.34580000000000;0.00215328000000000;-0.00134580000000000;-0.0174954000000000;0.00370501600000000;3.58228000000000e-06;-0.000188221360000000;0.00346547344800000;-3.58964000000000e-05;0.00184487804800000;0.245000000000000;0.00475275500000000;-4.01800000000000e-05;-0.0298802000000000;0.00504095578152000;5.29451820000000e-07;0.000588972999800000;0.00517846488624500;-4.97935280000000e-06;0.000194923283765000;0.131856000000000;0.00338368867200000;0;-0.00554797305600000;0.000286842514304256;0;0.000112106084563072;0.000487413733005120;4.47000000000000e-10;0.000224543218700864;0.0541526795194433;9.69221017872543e-11;0.0585168149031637;0.0189019411093441;0.101431488795825;0.00170374147663682;0.117323384204510;0.0342703864052806;0.00870769645781322;0.0423461865159219];


%% 左腿

% Bc = [0;0;0;0;0];
% B = [0;0;0;0;0];

for i = 1:1:n_data
%     tau_est(:,i) = K(:,:,i)*X + Bc.*sign(data_qd(:,i)) + B.*data_qd(:,i);
    tau_est(:,i) = K_aug(:,:,i)*X ;
    tau_est_raw(:,i) = K_aug(:,:,i)*X_raw;
end


%计算均方误差
error_vec = zeros(n_data,1);
for i=1:n_data
%         error_norm1(i) = norm(K(:,:,i)*X-tau(:,i),2);

%     error_norm_total(i) = norm(tau_est(:,i)-data_tau(:,i),2);   
%     error_norm_total_raw(i) = norm(tau_est_raw(:,i)-data_tau(:,i),2);  
%     error_norm1(i) = norm(tau_est(1,i)-data_tau(1,i),2); 
%     error_norm2(i) = norm(tau_est(2,i)-data_tau(2,i),2); 
%     error_norm3(i) = norm(tau_est(3,i)-data_tau(3,i),2); 
%     error_norm4(i) = norm(tau_est(4,i)-data_tau(4,i),2); 
%     error_norm5(i) = norm(tau_est(5,i)-data_tau(5,i),2); 

    error_norm_total(i) = norm(tau_est(:,i)-data_tau_filted_(:,i),2);   
    error_norm_total_raw(i) = norm(tau_est_raw(:,i)-data_tau_filted_(:,i),2);  
    error_norm1(i) = norm(tau_est(1,i)-data_tau_filted_(1,i),2); 
    error_norm2(i) = norm(tau_est(2,i)-data_tau_filted_(2,i),2); 
    error_norm3(i) = norm(tau_est(3,i)-data_tau_filted_(3,i),2); 
    error_norm4(i) = norm(tau_est(4,i)-data_tau_filted_(4,i),2); 
    error_norm5(i) = norm(tau_est(5,i)-data_tau_filted_(5,i),2); 
end
MSE = sum(error_norm_total*error_norm_total')
MSE_raw = sum(error_norm_total_raw*error_norm_total_raw')

MSE1 = sum(error_norm1*error_norm1');
MSE2 = sum(error_norm2*error_norm2');
MSE3 = sum(error_norm3*error_norm3');
MSE4 = sum(error_norm4*error_norm4');
MSE5 = sum(error_norm5*error_norm5');
joint_error =[MSE1 MSE2 MSE3 MSE4 MSE5 ]'/n_data

for i =1:nb
    figure (i)
    T = t:t:t*n_data;
    plot(T,data_tau(i,1:n_data),'-.b')
    hold on


    plot(T,tau_est(i,1:n_data),'r')
    hold on
    
%     plot(T,tau_est_raw(i,1:n_data),'g')
    hold on
    
%     plot(T,0.01*data_qdd(i,1:n_data),'--g')
    xlabel('t(s) 蓝色为测量值 红色为估计值' );
    ylabel('joint torque(Nm)')
end

